package physical.equation;

import simulation.Statics;
import base.Cell;
import base.Particle;
import physical.equation.math.Kernel;
import physical.equation.math.VecTool;
import physical.equation.math.MatrixTool;
import physical.equation.material.*;

public class SolidSPH
{
	private static double[] rangeX = new double[] { -10, +10};
	private static double[] rangeY = new double[] { -10, +10};
	private static double strength = 1e6;
	private static double lastAdd = 0;
	
	public static void doRefresh()
	{
		//if(true || rangeX[0] < -40)
		//{rangeX[0] += 2000 * Statics.DT;}
		
		/*
		for(int i = 0; i < Statics.particleArray.particleCount; i++)
		{
			Particle p = Statics.particleArray.particles[i];
		
			p.spd[0] = -5.0 * p.pos[1];
			p.spd[1] = +5.0 * p.pos[0];
		}
		*/
	}
	
	public static void calcNeighborGWDensityRefresh(Particle _particle)
	{
		// find neighbors of the particle
		int particleNeighborCount = 0;
		int cellNeighborCount = _particle.cell.neighborCount;
		Cell[] neighbors = _particle.cell.neighbors;
		double[] pos = _particle.pos;
		for(int i = 0; i < cellNeighborCount; i++)
		{
			for(Particle p = neighbors[i].root; p != null; p = p.next)
			{
				if(VecTool.calcDist(VecTool.vecSub(p.pos, pos)) < Statics.H)
				{
					_particle.neighbors[particleNeighborCount] = p;
					particleNeighborCount++;
				}
			}
		}
		//for(int i = particleNeighborCount; i < _particle.neighborCount; i++)
		//{
		//	_particle.neighbors[i] = null;
		//}
		_particle.neighborCount = particleNeighborCount;
		
		// store value and gradients of kernel
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			double[] vecp = VecTool.vecSub(_particle.neighbors[i].pos, _particle.pos);
			double[] vecv = VecTool.vecSub(_particle.neighbors[i].spd, _particle.spd);
			double dist = VecTool.calcDist(vecp);
			
			_particle.poly6W[i] = Kernel.poly6(dist);
			_particle.spikyW[i] = Kernel.spiky(dist);
			
			double[] vec1 = Kernel.poly6G(vecp);
			double[] vec2 = Kernel.spikyG(vecp);
			_particle.poly6GW[i][0] = vec1[0];
			_particle.poly6GW[i][1] = vec1[1];
			_particle.spikyGW[i][0] = vec2[0];
			_particle.spikyGW[i][1] = vec2[1];
		}
		
		// calc density
		double density = 0;
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			density += _particle.neighbors[i].mass * _particle.poly6W[i];
		}
		_particle.density = density;
		
		//calc other properties
		_particle.refreshProperties();
	}
	
	public static void calcCorrection(Particle _particle)
	{
		if(true)
		{return;}
		
		double[][] X = new double[2][2];
		double[][] X_1 = new double[2][2];
		
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			double mul1 = _particle.neighbors[i].mass / _particle.neighbors[i].density;
			
			X[0][0] += mul1 * (_particle.neighbors[i].pos[0] - _particle.pos[0]) * _particle.spikyGW[i][0];
			X[0][1] += mul1 * (_particle.neighbors[i].pos[1] - _particle.pos[1]) * _particle.spikyGW[i][0];
			X[1][0] += mul1 * (_particle.neighbors[i].pos[0] - _particle.pos[0]) * _particle.spikyGW[i][1];
			X[1][1] += mul1 * (_particle.neighbors[i].pos[1] - _particle.pos[1]) * _particle.spikyGW[i][1];
		}
		X_1 = MatrixTool.inverse(X);
		if(X_1[0][0] * 0 != 0)
		{
			System.out.println(_particle.id);
			System.out.println(X[0][0] + "\t" + X[0][1]);
			System.out.println(X[1][0] + "\t" + X[1][1]);
			System.out.println("");
			System.out.println("");
			
			Statics.isSimulating = false;
		}
		
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			double[] vec2 = new double[2];
			
			vec2[0] = X_1[0][0] * _particle.spikyGW[i][0] + X_1[0][1] * _particle.spikyGW[i][1];
			vec2[1] = X_1[1][0] * _particle.spikyGW[i][0] + X_1[1][1] * _particle.spikyGW[i][1];
			
			_particle.spikyGW[i][0] = vec2[0];
			_particle.spikyGW[i][1] = vec2[1];
		}
	}
	
	public static void calcAccelerationWork(Particle _particle)
	{
		double[] aclPressure = new double[2];
		double[] correction = new double[2];
		double workPressure = 0;
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			//calc correction factor for Gradient P
			double[] vecp = VecTool.vecSub(_particle.neighbors[i].pos, _particle.pos);
			double[] vecv = VecTool.vecSub(_particle.neighbors[i].spd, _particle.spd);
			double mul1 = _particle.neighbors[i].mass / _particle.neighbors[i].density;
			correction[0] += -vecp[0] * _particle.poly6GW[i][0] * mul1;
			correction[1] += -vecp[1] * _particle.poly6GW[i][1] * mul1;
		}
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			double[] vecp = VecTool.vecSub(_particle.neighbors[i].pos, _particle.pos);
			double[] vecv = VecTool.vecSub(_particle.neighbors[i].spd, _particle.spd);
			double a1 = _particle.neighbors[i].pressure / (_particle.neighbors[i].density * _particle.neighbors[i].density);
			double a2 = _particle.pressure / (_particle.density * _particle.density);
			double mul1 = _particle.neighbors[i].mass * (a1 + a2);
			
			double acl0 = mul1 * _particle.spikyGW[i][0];// / correction[0];
			double acl1 = mul1 * _particle.spikyGW[i][1];// / correction[1];
			aclPressure[0] += acl0;
			aclPressure[1] += acl1;
			
			workPressure += 0.5 * (acl0 * vecv[0] + acl1 * vecv[1]);
		}
		
		
		double[] aclViscosity = new double[2];
		double workViscosity = 0;
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			if(_particle.neighbors[i] != _particle)
			{
				double[] vecp = VecTool.vecSub(_particle.neighbors[i].pos, _particle.pos);
				double[] vecv = VecTool.vecSub(_particle.neighbors[i].spd, _particle.spd);
				double dist2 = VecTool.calcDist2(vecp);
				double mul1 = (-2 * _particle.viscosityMu * _particle.neighbors[i].mass) * VecTool.vecDot(vecp, _particle.poly6GW[i]) / (_particle.neighbors[i].density * dist2);
				
				double acl0 = mul1 * vecv[0];
				double acl1 = mul1 * vecv[1];
				aclViscosity[0] += acl0;
				aclViscosity[1] += acl1;
				
				workViscosity += 0.5 * (acl0 * vecv[0] + acl1 * vecv[1]);
			}
		}
		
		
		double[] aclArtViscosity = new double[2];
		double workArtViscosity = 0;
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			double[] vecp = VecTool.vecSub(_particle.neighbors[i].pos, _particle.pos);
			double[] vecv = VecTool.vecSub(_particle.neighbors[i].spd, _particle.spd);
			double dot1 = VecTool.vecDot(vecp, vecv);
			if(dot1 < 0)
			{
				double dist = VecTool.calcDist(vecp);
				
				double alpha = 0.1;
				double beta = 2;
				
				double chi = Statics.H * dot1 / (dist * dist + 0.01 * Statics.H * Statics.H);
				double pi = (-0.5 * alpha * (_particle.soundSpeed + _particle.neighbors[i].soundSpeed) * chi + beta * chi * chi) / (0.5 * (_particle.density + _particle.neighbors[i].density));
				
				double mul1 = _particle.neighbors[i].mass * pi;
				
				double acl0 = mul1 * _particle.spikyGW[i][0];
				double acl1 = mul1 * _particle.spikyGW[i][1];
				aclArtViscosity[0] += acl0;
				aclArtViscosity[1] += acl1;
				
				workArtViscosity += 0.5 * (acl0 * vecv[0] + acl1 * vecv[1]);
			}
		}
		
		double[] aclStress = new double[2];
		double workStress = 0;
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			if(_particle.neighbors[i] != _particle)
			{
				double[] vecp = VecTool.vecSub(_particle.neighbors[i].pos, _particle.pos);
				double[] vecv = VecTool.vecSub(_particle.neighbors[i].spd, _particle.spd);
				{
					double[] vect = VecTool.vecAdd(_particle.neighbors[i].stressTensor[0], _particle.stressTensor[0]);
					double mul1 = -_particle.neighbors[i].mass * VecTool.vecDot(vect, _particle.spikyGW[i]) / (_particle.neighbors[i].density);
					
					double acl0 = mul1 / _particle.density;
					aclStress[0] += acl0;
				}
				{
					double[] vect = VecTool.vecAdd(_particle.neighbors[i].stressTensor[1], _particle.stressTensor[1]);
					double mul1 = -_particle.neighbors[i].mass * VecTool.vecDot(vect, _particle.spikyGW[i]) / (_particle.neighbors[i].density);
					
					double acl1 = mul1 / _particle.density;
					aclStress[1] += acl1;
				}
			}
		}
		
		double[] aclArtStress = new double[2];
		double workArtStress = 0;
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			if(_particle.neighbors[i] != _particle)
			{
				double[] vecp = VecTool.vecSub(_particle.neighbors[i].pos, _particle.pos);
				double[] vecv = VecTool.vecSub(_particle.neighbors[i].spd, _particle.spd);
		
				double f = _particle.spikyW[i] / Kernel.spiky(0.65 * Statics.H);
				double r00, r01, r10, r11;
				r00 = _particle.correctionR[0][0] + _particle.neighbors[i].correctionR[0][0];
				r01 = _particle.correctionR[0][1] + _particle.neighbors[i].correctionR[0][1];
				r10 = _particle.correctionR[1][0] + _particle.neighbors[i].correctionR[1][0];
				r11 = _particle.correctionR[1][1] + _particle.neighbors[i].correctionR[1][1];
		
				double mul1 = -_particle.neighbors[i].mass * (f * f);
				aclArtStress[0] += mul1 * (r00 * _particle.spikyGW[i][0] + r01 * _particle.spikyGW[i][1]);
				aclArtStress[1] += mul1 * (r10 * _particle.spikyGW[i][0] + r11 * _particle.spikyGW[i][1]);
			}
		}
		/*
		*/
		
		
		double[] gravity = _particle.gravity;
		
		
		//heat transfer
		double workHeat = 0;
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			if(_particle.neighbors[i] != _particle)
			{
				double[] vecp = VecTool.vecSub(_particle.neighbors[i].pos, _particle.pos);
				double dt = _particle.neighbors[i].temperature - _particle.temperature;
				double dist = VecTool.calcDist(vecp);
				
				double mul1 = -dt * 2 * _particle.heatKappa * _particle.neighbors[i].mass / (_particle.neighbors[i].density * _particle.density * (dist * dist));
				workHeat += mul1 * VecTool.vecDot(_particle.poly6GW[i], vecp);
			}
		}
		
		//acceleration
		double[] acl = new double[2];
		acl[0] = 1.0 * aclPressure[0] + 1.0 * aclViscosity[0] + 1.0 * aclArtViscosity[0] + 1.0 * aclStress[0] + 1.0 * aclArtStress[0] + 1.0 * gravity[0];
		acl[1] = 1.0 * aclPressure[1] + 1.0 * aclViscosity[1] + 1.0 * aclArtViscosity[1] + 1.0 * aclStress[1] + 1.0 * aclArtStress[1] + 1.0 * gravity[1];
		
		//acl[0] = 1.0 * aclStress[0];
		//acl[1] = 1.0 * aclStress[1];
		_particle.acl[0] = acl[0];
		_particle.acl[1] = acl[1];
		
		//work
		double work = 1.0 * workPressure + 1.0 * workViscosity + 1.0 * workArtViscosity + 1.0 * workStress + 1.0 * workArtStress;
		_particle.work = work;
	}
	
	
	public static void calcStress(Particle _particle)
	{
		double[][] vD = new double[2][2];
		double[][] eD = new double[2][2];
		double[][] wD = new double[2][2];
		
		double[][] X = new double[2][2];
		double[][] X_1 = new double[2][2];
		double[][] vDY = new double[2][2];
		
		/*
		*/
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			double mul1 = _particle.neighbors[i].mass / _particle.neighbors[i].density;
			
			X[0][0] += mul1 * (_particle.neighbors[i].pos[0] - _particle.pos[0]) * _particle.spikyGW[i][0];
			X[0][1] += mul1 * (_particle.neighbors[i].pos[0] - _particle.pos[0]) * _particle.spikyGW[i][1];
			X[1][0] += mul1 * (_particle.neighbors[i].pos[1] - _particle.pos[1]) * _particle.spikyGW[i][0];
			X[1][1] += mul1 * (_particle.neighbors[i].pos[1] - _particle.pos[1]) * _particle.spikyGW[i][1];
		}
		X_1 = MatrixTool.inverse(X);
		
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			double mul1 = _particle.neighbors[i].mass / _particle.neighbors[i].density;
			
			vDY[0][0] += mul1 * (_particle.neighbors[i].spd[0] - _particle.spd[0]) * _particle.spikyGW[i][0];
			vDY[0][1] += mul1 * (_particle.neighbors[i].spd[1] - _particle.spd[1]) * _particle.spikyGW[i][0];
			vDY[1][0] += mul1 * (_particle.neighbors[i].spd[0] - _particle.spd[0]) * _particle.spikyGW[i][1];
			vDY[1][1] += mul1 * (_particle.neighbors[i].spd[1] - _particle.spd[1]) * _particle.spikyGW[i][1];
		}
		{
			vD[0][0] = X_1[0][0] * vDY[0][0] + X_1[0][1] * vDY[1][0];
			vD[0][1] = X_1[0][0] * vDY[0][1] + X_1[0][1] * vDY[1][1];
			vD[1][0] = X_1[1][0] * vDY[0][0] + X_1[1][1] * vDY[1][0];
			vD[1][1] = X_1[1][0] * vDY[0][1] + X_1[1][1] * vDY[1][1];
		}
		
		/*
		for(int i = 0; i < _particle.neighborCount; i++)
		{
			{
				double a1 = (_particle.neighbors[i].spd[0] - _particle.spd[0]) / (_particle.neighbors[i].density * _particle.neighbors[i].density);
				double a2 = (_particle.spd[0] - _particle.spd[0]) / (_particle.density * _particle.density);
				double mul1 = -_particle.density * _particle.neighbors[i].mass * (a1 + a2);
				vD[0][0] += mul1 * _particle.spikyGW[i][0];
				vD[1][0] += mul1 * _particle.spikyGW[i][1];
			}
			{
				double a1 = (_particle.neighbors[i].spd[1] - _particle.spd[1]) / (_particle.neighbors[i].density * _particle.neighbors[i].density);
				double a2 = (_particle.spd[1] - _particle.spd[1]) / (_particle.density * _particle.density);
				double mul1 = -_particle.density * _particle.neighbors[i].mass * (a1 + a2);
				vD[0][1] += mul1 * _particle.spikyGW[i][0];
				vD[1][1] += mul1 * _particle.spikyGW[i][1];
			}
		}
		*/
		
		eD[0][0] = 0.5 * (vD[0][0] + vD[0][0]);
		eD[0][1] = 0.5 * (vD[0][1] + vD[1][0]);
		eD[1][0] = 0.5 * (vD[1][0] + vD[0][1]);
		eD[1][1] = 0.5 * (vD[1][1] + vD[1][1]);
		
		wD[0][0] = 0.5 * (vD[0][0] - vD[0][0]);
		wD[0][1] = 0.5 * (vD[0][1] - vD[1][0]);
		wD[1][0] = 0.5 * (vD[1][0] - vD[0][1]);
		wD[1][1] = 0.5 * (vD[1][1] - vD[1][1]);
		
		double[][] sT = _particle.stressTensor;
		double[][] stressTensorD = new double[2][2];
		
		//stressTensorD[0][0] = (eD[0][0] - eD[1][1]) * _particle.stressMu;
		//stressTensorD[0][1] = (eD[0][1] + eD[1][0]) * _particle.stressMu;
		//stressTensorD[1][0] = (eD[1][0] + eD[0][1]) * _particle.stressMu;
		//stressTensorD[1][1] = (eD[1][1] - eD[0][0]) * _particle.stressMu;
		
		stressTensorD[0][0] = (eD[0][0] + eD[0][0]) * _particle.stressMu;
		stressTensorD[0][1] = (eD[0][1] + eD[1][0]) * _particle.stressMu;
		stressTensorD[1][0] = (eD[1][0] + eD[0][1]) * _particle.stressMu;
		stressTensorD[1][1] = (eD[1][1] + eD[1][1]) * _particle.stressMu;
		
		stressTensorD[0][0] += (eD[0][0] + eD[1][1]) * _particle.stressLambda;
		stressTensorD[1][1] += (eD[0][0] + eD[1][1]) * _particle.stressLambda;
		
		stressTensorD[0][0] -= (wD[0][0] * sT[0][0] + wD[0][1] * sT[1][0]) - (sT[0][0] * wD[0][0] + sT[0][1] * wD[1][0]);
		stressTensorD[0][1] -= (wD[0][0] * sT[0][1] + wD[0][1] * sT[1][1]) - (sT[0][0] * wD[0][1] + sT[0][1] * wD[1][1]);
		stressTensorD[1][0] -= (wD[1][0] * sT[0][0] + wD[1][1] * sT[1][0]) - (sT[1][0] * wD[0][0] + sT[1][1] * wD[1][0]);
		stressTensorD[1][1] -= (wD[1][0] * sT[0][1] + wD[1][1] * sT[1][1]) - (sT[1][0] * wD[0][1] + sT[1][1] * wD[1][1]);
		
		_particle.vD[0][0] = vD[0][0];
		_particle.vD[0][1] = vD[0][1];
		_particle.vD[1][0] = vD[1][0];
		_particle.vD[1][1] = vD[1][1];
		
		_particle.stressTensorD[0][0] = stressTensorD[0][0];
		_particle.stressTensorD[0][1] = stressTensorD[0][1];
		_particle.stressTensorD[1][0] = stressTensorD[1][0];
		_particle.stressTensorD[1][1] = stressTensorD[1][1];
		
		//correction R
		double theta = 0.5 * Math.atan(2 * _particle.stressTensor[0][1] / (_particle.stressTensor[0][0] - _particle.stressTensor[1][1]));
		if(Double.isNaN(theta) == false)
		{
			double epsilon = 0.01;
		
			double s = Math.sin(theta);
			double c = Math.cos(theta);
			double sigmaxx = c * c * _particle.stressTensor[0][0] + 2 * c * s * _particle.stressTensor[0][1] + s * s * _particle.stressTensor[1][1];
			double sigmayy = s * s * _particle.stressTensor[0][0] + 2 * c * s * _particle.stressTensor[0][1] + c * c * _particle.stressTensor[1][1];
		
			double rxx = -epsilon * sigmaxx / (_particle.density * _particle.density);
			double ryy = -epsilon * sigmayy / (_particle.density * _particle.density);
			if(rxx > 0)
			{rxx = 0;}
			if(ryy > 0)
			{ryy = 0;}
		
			_particle.correctionR[0][0] = c * c * rxx + s * s * ryy;
			_particle.correctionR[0][1] = s * c * (rxx - ryy);
			_particle.correctionR[1][0] = s * c * (rxx - ryy);
			_particle.correctionR[1][1] = s * s * rxx + c * c * ryy;
		}
		else
		{
			_particle.correctionR[0][0] = 0;
			_particle.correctionR[0][1] = 0;
			_particle.correctionR[1][0] = 0;
			_particle.correctionR[1][1] = 0;
		}
	}
	
	
	public static void calcIterate(Particle _particle)
	{
		double[] force = new double[2];
		force[0] += (_particle.pos[0] < rangeX[0]) ? (rangeX[0] - _particle.pos[0]) : 0;
		force[0] += (_particle.pos[0] > rangeX[1]) ? (rangeX[1] - _particle.pos[0]) : 0;
		force[1] += (_particle.pos[1] < rangeY[0]) ? (rangeY[0] - _particle.pos[1]) : 0;
		force[1] += (_particle.pos[1] > rangeY[1]) ? (rangeY[1] - _particle.pos[1]) : 0;
		
		//force[0] += 0.005 * _particle.pos[0];
		
		
		_particle.acl[0] += strength * force[0];
		_particle.acl[1] += strength * force[1];
		
		_particle.spd[0] += _particle.acl[0] * Statics.DT;
		_particle.spd[1] += _particle.acl[1] * Statics.DT;
		
		_particle.pos[0] += _particle.spd[0] * Statics.DT;
		_particle.pos[1] += _particle.spd[1] * Statics.DT;
		
		
		_particle.energy += _particle.work * Statics.DT;
		
		_particle.stressTensor[0][0] += _particle.stressTensorD[0][0] * Statics.DT;
		_particle.stressTensor[0][1] += _particle.stressTensorD[0][1] * Statics.DT;
		_particle.stressTensor[1][0] += _particle.stressTensorD[1][0] * Statics.DT;
		_particle.stressTensor[1][1] += _particle.stressTensorD[1][1] * Statics.DT;
	}
}